import numpy as np
from algorithm.dataprocessing import utilities as utl
import UR5 as Rb

filter = utl.Filter()

class FTSensor():
    def __init__(self, filter_type):
        self.filter_type = filter_type
        self.new_force = np.zeros(3)
        self.new_torque = np.zeros(3)
        self.old_force = np.zeros(3)
        self.old_torque = np.zeros(3)
        self.wrench_safety = np.zeros(6)
        self.PoseTool2Sensor = np.zeros(6)
        self._adj_tool_2_sensor = np.zeros((6, 6))

        self._Foffset = np.zeros(3)
        self._Toffset = np.zeros(3)
        self._Gravity = np.zeros(3)
        self._Pcom = np.zeros(3)
    # get wrench data projected on sensor frame
    def getWrenchSensor(self):
        wrench = np.zeros(6)
        wrench[0] = self.new_force[0]
        wrench[1] = self.new_force[1]
        wrench[2] = self.new_force[2]
        wrench[3] = self.new_torque[0]
        wrench[4] = self.new_torque[1]
        wrench[5] = self.new_torque[2]

        if self.filter_type == Rb.lowpass_filter:
            wrench = filter.LowPassFilter(wrench)
        return wrench

    # get wrench data projected on tool frame
    def getWrenchTool(self):
        wrench_sensor = self.getWrenchSensor()
        wrench_T, wrench_S = np.zeros(6), np.zeros(6)
        wrench_S = wrench_sensor
        wrench_T = self._adj_tool_2_sensor * wrench_S
        return wrench_T

    def getWrenchNetTool(self, qtn):
        wrench_T = self.getWrenchTool()

        GinF = np.dot(np.transpose(qtn.rotation_matrix), self._Gravity)
        GinT = -np.cross(self._Pcom, GinF)
        wrench_net_T = np.zeros(6)
        wrench_net_T[0] = wrench_T[0] + self._Foffset[0] - GinF[0]
        wrench_net_T[1] = wrench_T[1] + self._Foffset[1] - GinF[1]
        wrench_net_T[2] = wrench_T[2] + self._Foffset[2] - GinF[2]

        wrench_net_T[3] = wrench_T[0] + self._Toffset[0] - GinT[0]
        wrench_net_T[4] = wrench_T[1] + self._Toffset[1] - GinT[1]
        wrench_net_T[5] = wrench_T[2] + self._Toffset[2] - GinT[2]

        for i in range(6):
            if abs(wrench_net_T[i]>self.wrench_safety[i]):
                return 1   # danger
        return wrench_net_T



